#ifndef _SIFT_TRACKER_H_
#define _SIFT_TRACKER_H_

#include "Tracker.h"
#include "DATObject.h"
#include <iostream>
#include <iomanip>
#include <vector>
#include <utility>
//extern "C"
//{
	//C library of Rob Hess
	//Seems good, if problems, kill the bastard.
	#include "sift.h"
	#include "imgfeatures.h"
	#include "kdtree.h"
	#include "xform.h"
	#include "match_utils.h"
//}

/* the maximum number of keypoint NN candidates to check during BBF search */
//200 default
#define KDTREE_BBF_MAX_NN_CHKS 200

/* threshold on squared ratio of distances between NN and 2nd NN */
//0.49 default
#define NN_SQ_DIST_RATIO_THR 0.49

//TODO::Create a DATSIFT class and derive DATObjectSIFT and DATFrameSIFT from it

#define SIMILAR_PT_THRESHOLD 0.5

#define MIN_DIST_CENTER		0.5	//in pixel. The minimum distance from the center a point must be to be used in the box ratio.
#define MAX_DIST_RATIO		1.1	//AREA in distance
#define MAX_SIZE_RATIO		2	//if its higher then that, there's a problem. (CHEESY) src/dest
#define MIN_SIZE_RATIO		1/MAX_SIZE_RATIO	//if its lower then that, there's a problem.

struct DATObjectSIFT : DATObject
{
	DATObjectSIFT()
	{
		mSIFTFeatures = NULL;
		mKDRoot = NULL;
		mSIFTFeaturesFramePos = -1;	//this thing should always be updated...
		mNbOfFeatures = -1;
		mLastWasDetected = false;
		mLastPosition.x = mLastPosition.y = mLastPosition.width = mLastPosition.height = -1;

		mIntvls = SIFT_INTVLS;
		mSigma = SIFT_SIGMA;
		mContrThr =	SIFT_CONTR_THR;
		mCurvThr = SIFT_CURV_THR;
		mImgDbl = SIFT_IMG_DBL;

		mDescrWidth = SIFT_DESCR_WIDTH;
		mDescrHistBins = SIFT_DESCR_HIST_BINS;
		
	};
	virtual ~DATObjectSIFT()
	{
		if(mSIFTFeatures)
			free(mSIFTFeatures);	//sorry :(
		if(mKDRoot)
			kdtree_release(mKDRoot);
	};
	//SIFT parameters
	int mIntvls;		//Intervals of octave ?
	double mSigma;		//Sigma of the first pyramids
	double mContrThr;	//a threshold on the value of the scale space function \f$\left|D(\hat{x})\right|\f$, 
						//where \f$\hat{x}\f$ is a vector specifying feature location and scale, used to reject 
						//unstable features; i.e. : Don't know
	int mCurvThr;		//how much curve is permitted
	int mImgDbl;		//Double the image before pyramids
	int mDescrWidth;	//Don't touch those ?
	int mDescrHistBins; //Don't touch those ?

	bool calculateSIFTFeatures();
	bool buildKDTree();
	void clearSIFT();
	int mSIFTFeaturesFramePos;
	feature* mSIFTFeatures;
	kd_node* mKDRoot;
	int mNbOfFeatures;
	
	
};

struct DATFrameSIFT : DATFrame
{
	DATFrameSIFT()
	{
		mSIFTFeatures = NULL;
		mKDRoot = NULL;
		mNbOfFeatures = 0;

		mIntvls = SIFT_INTVLS;
		mSigma = SIFT_SIGMA;
		mContrThr =	SIFT_CONTR_THR;
		mCurvThr = SIFT_CURV_THR;
		mImgDbl = SIFT_IMG_DBL;

		mDescrWidth = SIFT_DESCR_WIDTH;
		mDescrHistBins = SIFT_DESCR_HIST_BINS;

		mLastROI = cv::Rect(-1,-1,-1,-1);
	};
	DATFrameSIFT(cv::Mat& inImage, int inFramePos)
	{
		mActualFrame = inImage;
		mFramePos = inFramePos;

		mSIFTFeatures = NULL;
		mKDRoot = NULL;
		mNbOfFeatures = 0;

		mIntvls = SIFT_INTVLS;
		mSigma = SIFT_SIGMA;
		mContrThr =	SIFT_CONTR_THR;
		mCurvThr = SIFT_CURV_THR;
		mImgDbl = SIFT_IMG_DBL;

		mDescrWidth = SIFT_DESCR_WIDTH;
		mDescrHistBins = SIFT_DESCR_HIST_BINS;

		mLastROI = cv::Rect(-1,-1,-1,-1);
	}

	virtual ~DATFrameSIFT()
	{
		if(mSIFTFeatures)
			free(mSIFTFeatures);	//sorry :(
		if(mKDRoot)
			kdtree_release(mKDRoot);
	};
	//SIFT parameters
	int mIntvls;		//Intervals of octave ?
	double mSigma;		//Sigma of the first pyramids
	double mContrThr;	//a threshold on the value of the scale space function \f$\left|D(\hat{x})\right|\f$, 
						//where \f$\hat{x}\f$ is a vector specifying feature location and scale, used to reject 
						//unstable features; i.e. : Don't know
	int mCurvThr;		//how much curve is permitted
	int mImgDbl;		//Double the image before pyramids
	int mDescrWidth;	//Don't touch those ?
	int mDescrHistBins; //Don't touch those ?

	bool calculateSIFTFeatures();
	bool buildKDTree();
	void clearSIFT();

	feature* mSIFTFeatures;
	kd_node* mKDRoot;
	int mNbOfFeatures;

	cv::Rect	mLastROI;
};


class SIFTTracker : public Tracker
{
public:
	SIFTTracker() : Tracker()
	{
		mFlag = TRACK_IN_FRAME_DEFAULT;

		mNbOfNeighborsToFind = 2;
		mMaxNNChks = KDTREE_BBF_MAX_NN_CHKS;
		mThreshold = NN_SQ_DIST_RATIO_THR;
	};
	SIFTTracker(DATObject* inDATObject) : Tracker(inDATObject)
	{
		mPriorConfidence = DEFAULT_TRACKER_PRIOR_CONFIDENCE;
		mFlag = TRACK_IN_FRAME_DEFAULT;

		mNbOfNeighborsToFind = 2;
		mMaxNNChks = KDTREE_BBF_MAX_NN_CHKS;
		mThreshold = NN_SQ_DIST_RATIO_THR;
	};
	SIFTTracker(DATObject* inDATObject, float inPriorConfidence) : Tracker(inDATObject,inPriorConfidence)
	{
		mFlag = TRACK_IN_FRAME_DEFAULT;

		mNbOfNeighborsToFind = 2;
		mMaxNNChks = KDTREE_BBF_MAX_NN_CHKS;
		mThreshold = NN_SQ_DIST_RATIO_THR;
	};
	virtual ~SIFTTracker()
	{

	}
	//The confidence about the answer (if its "true" then, its the confidence of the "true" answer... 
	//otherwise, its the confidence of the false answer... the false confidence only have sense for "compareObjects")
	virtual bool compareObjects(DATObject* inDATObject, double* outConfidence);
	virtual bool findObjectInFrame(DATFrame* inDATFrame, pair<cv::Point, double>& outRectTransformFound, double* outConfidence);

	//
	virtual void update(DATObject* inDATObject, double inAliveConfidenceThreshold = ALIVE_CONFIDANCE, int inLifeCounterThreshold = ALIVE);
	virtual void update(std::pair<cv::Point,double>& inRectTransformFound, cv::Mat &inFrame, unsigned int inFramePos);

	//*** Set/get ***//
	void setNbOfNeighborsToFind(int inNbOfNeighborsToFind){mNbOfNeighborsToFind = inNbOfNeighborsToFind;};
	const int getNbOfNeighborsToFind()const{return mNbOfNeighborsToFind;};
	void setMaxNNChks(int inMaxNNChks){mMaxNNChks = inMaxNNChks;};
	const int getMaxNNChks()const{return mMaxNNChks;};
	void setThreshold(double inThreshold){mThreshold = inThreshold;};
	const double getThreshold()const{return mThreshold;};

	
protected:
	//SIFT match parameters
	int			mNbOfNeighborsToFind;
	int			mMaxNNChks;

	double		mThreshold;	// threshold on squared ratio of distances between NN and 2nd NN

	//Ok, lets try that shit...
	int			mLastDetectedObjectHistoryIdx;	//Use one pair of corner 
	//not sure it should be there, but its better then nothing
	vector<pair<cv::Point,cv::Point> > mLastMatches;	//Valid when H is valid

};

class cmpPair:public std::binary_function<pair<float,int>, pair<float,int>, bool>
{
public:
	inline bool operator()(const pair<float,int>& inPair1, const pair<float,int>& inPair2)
	{
		return inPair1.first<inPair2.first;
	}
};

#endif
